/*
 * SG90舵机引脚连接说明
 * | 功能名称      | 端口号    | 复用功能 | 板载引脚 |
 * |:-------------|:----------|:---------|:---------|
 * | VCC （红色）  | 5V        | -        | -        |
 * | GND（黑色）    | GND       | -        | -        |
 * |  信号线（黄色） |  PWM4      |      | J12-40   |
 *
 */
#include "../board/sdk_project_config.h"
#include "peripherals_uart_2_config.h"
#include "pwm_driver.h"
#include <stdio.h>
#include <string.h>

#define SERVO_PERIOD_TICKS 40000U // 20ms, 2MHz时钟下
#define SERVO_MIN_PULSE_TICKS 2000U  // 0.5ms
#define SERVO_MAX_PULSE_TICKS 40000U // 20.0ms，极限测试

#define SERVO_PWM_INST 4 // 只用PWM4

static pwm_state_t g_servoState;
static pwm_config_t g_servoConfig;

uint32_t angle_to_ticks(int angle) {
    if (angle < 0) angle = 0;
    if (angle > 180) angle = 180;
    return SERVO_MIN_PULSE_TICKS + (SERVO_MAX_PULSE_TICKS - SERVO_MIN_PULSE_TICKS) * angle / 180;
}

void servo_set_angle(int angle) {
    g_servoConfig.duty = angle_to_ticks(angle);
    PWM_DRV_Cfg(SERVO_PWM_INST, &g_servoState, &g_servoConfig);
}

void servo_init(void) {
    g_servoConfig.period = SERVO_PERIOD_TICKS;
    g_servoConfig.duty = angle_to_ticks(90); // 默认中位
    g_servoConfig.pwm_clk_src_cfg = G_CLK;
    g_servoConfig.pwm_polarity = S_RV_C_CMP;
    g_servoConfig.pwm_active_cfg = 0x1;
    g_servoConfig.prescaler = 0x4;
    g_servoConfig.repeat = ONCE;
    g_servoConfig.water_mark = 4;
    g_servoConfig.int_type = 0x7;
    g_servoConfig.callback = NULL;
    g_servoConfig.callbackParam = NULL;
    PWM_DRV_Init(SERVO_PWM_INST, &g_servoState, &g_servoConfig);
    PWM_DRV_Start(SERVO_PWM_INST);
}

void delay_ms(uint32_t ms) {
    for (volatile uint32_t i = 0; i < ms * 8000; ++i) {
        __asm__("nop");
    }
}

int main(void)
{
    CLOCK_SYS_Init(g_pstClockManConfigsArr[0]);
    PINS_DRV_Init(NUM_OF_CONFIGURED_PINS, g_stPinmuxConfigArr);
    UART_DRV_Init(INST_UART_2, &g_stUartState_2, &g_stUart2UserConfig0);
    UART_DRV_SendDataBlocking(INST_UART_2, (uint8_t*)"PWM4单路舵机极限测试，自动往返演示\r\n", 52, 500U);
    servo_init();
    while (1) {
        // 0->180
        for (int angle = 0; angle <= 180; angle += 5) {
            servo_set_angle(angle);
            delay_ms(20);
        }
        // 180->0
        for (int angle = 180; angle >= 0; angle -= 5) {
            servo_set_angle(angle);
            delay_ms(20);
        }
    }
}
